
#include <base/Svar/Scommand.h>
#include <base/Svar/VecParament.h>
#include <base/utils/utils.h>
#include <cv/highgui/VideoPOS_Manager.h>
#include <uav/UAS.h>
#include <uav/VirtualUAV.h>

#ifdef USE_INTEGRATED_SLAM
#include "SystemGSLAM.h"
#include "SystemOSLAM.h"
#endif

#include "DataManager.h"
#include "ComManager.h"
#include "utils_system.h"


using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

DataManager::DataManager(std::string systemName)
{
    SvarWithType<DataManager*>::instance()["DataManager.ptr"] = this;

    m_systemName = systemName;

    // create objs
    m_comManager            = SPtr<ComManager>(new ComManager);
    m_uasManager            = SPtr<UAS_Manager>(new UAS_Manager);
    m_vuavManager           = SPtr<VirtualUAV_Manager>(new VirtualUAV_Manager);
    m_joystick              = SPtr<HAL_JSComobo>(new HAL_JSComobo);
    //m_videoPOS_Manager      = SPtr<VideoPOS_Manager>(new VideoPOS_Manager);

    // create SLAM object
    #ifdef USE_INTEGRATED_SLAM
    int slamType = svar.GetInt("slamType", 0);
    switch( slamType )
    {
    case 1:
        m_systemSLAM = SPtr<SystemSLAM>(new SystemGSLAM);
        break;

    case 2:
        m_systemSLAM = SPtr<SystemSLAM>(new ORB_SLAM::SystemOSLAM);
        break;
    }
    #endif

    // start message TTS
    MessageTTS *mTTS = new MessageTTS;
    SvarWithType<MessageTTS*>::instance()["MessageTTS.ptr"] = mTTS;
    mTTS->run();

    m_isProjectOpened = 0;
}

DataManager::~DataManager()
{
    m_uasManager->stopTimer();
    m_vuavManager->stopTimer();

    close();

    SvarWithType<DataManager*>::instance()["DataManager.ptr"] = NULL;

    // stop and free MessageTTS
    MessageTTS *mTTS = SvarWithType<MessageTTS*>::instance().get_var("MessageTTS.ptr", NULL);
    if( mTTS )
    {
        mTTS->stop();
        delete mTTS;

        SvarWithType<MessageTTS*>::instance()["MessageTTS.ptr"] = NULL;
    }
}

int DataManager::open(InputDataType dt, std::string fn, int param)
{
    int ret = -1;

    m_dataType = dt;

    // close old connection first
    if( m_isProjectOpened ) close();

    // set com/uas managers
    m_comManager->setUASManager(m_uasManager.get());

    // open new connection (file)
    if( DATA_FILE == dt )
    {
        // project type
        svar.GetString("FastGCS.ProjectType", "File") = "File";

        // get file extname
        StringArray arrFN = path_splitext(fn);

        if( str_tolower(arrFN[1]) == ".cfg" )
        {
            string mvd_extname = svar.GetString("FastGCS.Mavlink.data_extname", ".mvd");
            m_comManager->open(ComManager::COM_DEVICE_FILE, arrFN[0]+mvd_extname, param);

            // open video
            // FIXME: better way?
            svar.GetString("Video.Type", "RTK") = "RTK";

            svar.GetInt("VideoPOS_Manager.syncTimeStamp", 0) = 1;
            //m_videoPOS_Manager->open(fn);

            // set Video and GPS to network
            // FIXME: better way?
            svar.GetString("Video.Type", "RTK") = "Network";
            svar.GetString("GPS.name", "GPS_IPC") = "GPS_IPC";
            svar.update();
            svar.save2file("./GSLAM_ext.cfg");
        }
        else if ( str_tolower(arrFN[1]) == ".mvd" )
        {
            m_comManager->open(ComManager::COM_DEVICE_FILE, fn, param);
        }

        // run client program
        if( 0 )
        {
            RunProgram *rp = NULL;
            if( ! SvarWithType<RunProgram*>::instance().exist("ClientProgram.ptr") )
            {
                rp = new RunProgram();
                SvarWithType<RunProgram*>::instance()["ClientProgram.ptr"] = rp;
            }
            else
            {
                rp = SvarWithType<RunProgram*>::instance()["ClientProgram.ptr"];
            }

            rp->run("./FastGCS act=test_VideoPOS_RX");
        }

        scommand.Call("MessageTTS Open file.");

        ret = 0;
    }

    // open new connection (live data)
    if ( DATA_LIVE == dt )
    {
        // project type
        svar.GetString("FastGCS.ProjectType", "Live") = "Live";

        // get autosave name
        DateTime dt(DateTime::DATETIME_LOCAL);
        string AutoSavePrjName = fmt::sprintf("%04d%02d%02d-%02d%02d%02d",
                                              dt.year, dt.month, dt.day,
                                              dt.hour, dt.min, dt.sec);
        svar.GetString("FastGCS.AutoSavePrjName", AutoSavePrjName) = AutoSavePrjName;

        // open communication
        m_comManager->open(ComManager::COM_DEVICE_UART, fn, param);

        // open video (FIXME: better way?)
        svar.GetString("Video.Type", "RTK") = "V4L";
        svar.GetInt("VideoPOS_Manager.syncTimeStamp", 0) = 0;           // FIXME: need sync PC time to GPS time
        //m_videoPOS_Manager->open("Video");

        // set Video and GPS to network
        // FIXME: better way?
        svar.GetString("Video.Type", "RTK") = "Network";
        svar.GetString("GPS.name", "GPS_IPC") = "GPS_IPC";
        svar.update();
        svar.save2file("./GSLAM_ext.cfg");

        scommand.Call("Open live capture.");

        ret = 0;
    }

    // open new connection (simulation)
    if ( DATA_SIMULATION == dt )
    {
        // project type
        svar.GetString("FastGCS.ProjectType", "Simulation") = "Simulation";

        // open communication
        m_comManager->open(ComManager::COM_DEVICE_IPC, fn, param);

        // open & set joystick
        m_joystick->open(svar.GetString("FastGCS.joystick", "Joystick.Madcatz_X-55"));
        m_vuavManager->setJoystick(m_joystick.get());

        // set UART
        m_vuavManager->setUART((VirtualUART*) m_comManager->getUART());

        scommand.Call("MessageTTS Begin simulation.");

        ret = 0;
    }

    // begin SLAM
    runSLAM();

    m_isProjectOpened = 1;

    return ret;
}

int DataManager::close(void)
{
    if( !m_isProjectOpened ) return -1;

    // stop SLAM
    stopSLAM();

    // close communications
    m_comManager->setUASManager(NULL);

    //m_videoPOS_Manager->close();
    m_comManager->close();
    m_uasManager->reset();
    m_vuavManager->reset();
    m_joystick->close();

    // stop client program
    if( SvarWithType<RunProgram*>::instance().exist("ClientProgram.ptr") )
    {
        RunProgram *rp = SvarWithType<RunProgram*>::instance()["ClientProgram.ptr"];

        rp->stop();
    }

    m_isProjectOpened = 0;

    return 0;
}

int DataManager::seek(double dt)
{
    if( m_dataType == DATA_FILE )
    {
        m_comManager->seek(dt);
    }

    return 0;
}


int DataManager::runSLAM(void)
{
    return -1;
}

int DataManager::stopSLAM(void)
{
    return -1;
}
